// 标准库头文件
#include <string>
#include <sstream>
#include <iostream>
#include <iomanip>
#include <cstring>
#include <unistd.h>
#include <sched.h>
#include <vector>
#include <tuple>

// 第三方库头文件
#include "rclcpp/rclcpp.hpp"
#include "geometry_msgs/msg/pose_stamped.hpp"
#include "geometry_msgs/msg/pose_array.hpp" // 新增PoseArray支持
#include "std_msgs/msg/string.hpp"
#include "sensor_msgs/msg/nav_sat_fix.hpp"
#include "tf2_ros/transform_broadcaster.h"
#include <tf2/LinearMath/Quaternion.h>
#include "GeographicLib/UTMUPS.hpp"
#include "rosgraph_msgs/msg/clock.hpp"
#include <nlohmann/json.hpp> // JSON解析库

using namespace std;
using namespace std::chrono_literals;

class TFConverterNode : public rclcpp::Node
{
public:
    bool hasClock = false;
    rosgraph_msgs::msg::Clock::SharedPtr clock_msg_; // 新增状态变量
    TFConverterNode() : Node("apm_gps_tf_node")
    {
        // 初始化参数
        this->declare_parameter<std::string>("apm_controller_namespace", "/apm_drone");
        apm_controller_namespace_ = this->get_parameter("apm_controller_namespace").get_parameter_value().get<std::string>();

        tf_broadcaster_ = std::make_shared<tf2_ros::TransformBroadcaster>(*this);

        // 设置订阅者
        subscription_local_ = this->create_subscription<geometry_msgs::msg::PoseStamped>(
            apm_controller_namespace_ + "/current_local_location", 10,
            std::bind(&TFConverterNode::local_location_callback, this, std::placeholders::_1));

        subscription_attitude_ = this->create_subscription<std_msgs::msg::String>(
            apm_controller_namespace_ + "/current_attitude", 10,
            std::bind(&TFConverterNode::attitude_callback, this, std::placeholders::_1));

        subscription_target_gps_ = this->create_subscription<sensor_msgs::msg::NavSatFix>(
            apm_controller_namespace_ + "/target_gps_location", 10,
            std::bind(&TFConverterNode::target_gps_callback, this, std::placeholders::_1));

        // 修改订阅主题为 current_GPS
        subscription_current_gps_ = this->create_subscription<sensor_msgs::msg::NavSatFix>(
            apm_controller_namespace_ + "/current_GPS", 10,
            std::bind(&TFConverterNode::current_gps_callback, this, std::placeholders::_1));

        // 多目标GPS订阅者
        subscription_mult_target_gps_ = this->create_subscription<std_msgs::msg::String>(
            apm_controller_namespace_ + "/mult_target_gps", 10,
            std::bind(&TFConverterNode::mult_target_gps_callback, this, std::placeholders::_1));

        // 多目标坐标发布者
        mult_pose_publisher_ = this->create_publisher<geometry_msgs::msg::PoseArray>("/mult_pose", 10);

        goal_publisher_ = this->create_publisher<geometry_msgs::msg::PoseStamped>("/goal_pose", 10);

        // 检查是否存在 /clock 主题并创建订阅者
        auto topics = this->get_topic_names_and_types();

        for (const auto &topic : topics)
        {
            if (topic.first == "/clock")
            {
                hasClock = true;
                break;
            }
        }

        if (hasClock)
        {
            RCLCPP_INFO(this->get_logger(), "/clock topic found. Creating subscription to /clock.");
            subscription_clock_ = this->create_subscription<rosgraph_msgs::msg::Clock>(
                "/clock", 10,
                std::bind(&TFConverterNode::sim_clock_callback, this, std::placeholders::_1));
        }
        else
        {
            RCLCPP_WARN(this->get_logger(), "/clock topic not found. Skipping /clock subscription.");
        }

        // 设置定时器
        timer_ = this->create_wall_timer(100ms, std::bind(&TFConverterNode::update_tf, this));
    }

private:
    // 参数
    std::string apm_controller_namespace_;

    // 订阅者
    rclcpp::Subscription<geometry_msgs::msg::PoseStamped>::SharedPtr subscription_local_;
    rclcpp::Subscription<std_msgs::msg::String>::SharedPtr subscription_attitude_;
    rclcpp::Subscription<sensor_msgs::msg::NavSatFix>::SharedPtr subscription_target_gps_;
    rclcpp::Subscription<sensor_msgs::msg::NavSatFix>::SharedPtr subscription_current_gps_; // 修改为 current_GPS
    rclcpp::Subscription<rosgraph_msgs::msg::Clock>::SharedPtr subscription_clock_;

    // 发布者
    rclcpp::Publisher<geometry_msgs::msg::PoseStamped>::SharedPtr goal_publisher_;

    // TF 广播器
    std::shared_ptr<tf2_ros::TransformBroadcaster> tf_broadcaster_;

    // 状态变量
    geometry_msgs::msg::PoseStamped::SharedPtr local_pose_;
    std::string attitude_;
    sensor_msgs::msg::NavSatFix::SharedPtr target_gps_;
    sensor_msgs::msg::NavSatFix::SharedPtr current_gps_; // 变量名修改为 current_gps_

    // 定时器
    rclcpp::TimerBase::SharedPtr timer_;

    rclcpp::Subscription<std_msgs::msg::String>::SharedPtr subscription_mult_target_gps_;
    rclcpp::Publisher<geometry_msgs::msg::PoseArray>::SharedPtr mult_pose_publisher_;
    std::vector<std::tuple<double, double, double>> mult_target_gps_coords_;

    void mult_target_gps_callback(const std_msgs::msg::String::SharedPtr msg)
    {
        try
        {
            mult_target_gps_coords_.clear();
            auto json_data = nlohmann::json::parse(msg->data);

            for (const auto &item : json_data)
            {
                double lat = item["lat"];
                double lon = item["lon"];
                double alt = item["alt"];
                mult_target_gps_coords_.emplace_back(lat, lon, alt);
            }
            // RCLCPP_INFO(this->get_logger(), "Received %zu multi-target GPS points",
            //             mult_target_gps_coords_.size());
        }
        catch (const std::exception &e)
        {
            RCLCPP_ERROR(this->get_logger(), "Multi-target GPS parsing failed: %s", e.what());
        }
    }

    // 本地位置回调
    void local_location_callback(const geometry_msgs::msg::PoseStamped::SharedPtr msg)
    {
        local_pose_ = msg;
    }

    // 姿态回调
    void attitude_callback(const std_msgs::msg::String::SharedPtr msg)
    {
        attitude_ = msg->data;
    }

    // 目标 GPS 回调
    void target_gps_callback(const sensor_msgs::msg::NavSatFix::SharedPtr msg)
    {
        target_gps_ = msg;
    }

    // 当前 GPS 回调（原 home_gps 回调修改）
    void current_gps_callback(const sensor_msgs::msg::NavSatFix::SharedPtr msg)
    {
        current_gps_ = msg;
    }

    // /clock 回调
    void sim_clock_callback(const rosgraph_msgs::msg::Clock::SharedPtr msg)
    {
        clock_msg_ = msg;
    }

    // 更新 TF 并发送消息
    void update_tf()
    {
        // 发布 odom 到 base_link 的 TF
        if (local_pose_ && !attitude_.empty())
        {
            geometry_msgs::msg::TransformStamped t;
            // 设置时间戳
            if (hasClock && clock_msg_)
            {
                t.header.stamp = clock_msg_->clock;
            }
            else
            {
                t.header.stamp = this->get_clock()->now();
            }
            t.header.frame_id = "odom";
            t.child_frame_id = "base_link";

            t.transform.translation.x = local_pose_->pose.position.y;
            t.transform.translation.y = local_pose_->pose.position.x;
            t.transform.translation.z = -local_pose_->pose.position.z;

            // 解析姿态字符串，例如 'Roll: 0.1, Pitch: 0.2, Yaw: 1.57'
            double roll, pitch, yaw;
            std::istringstream attitude_stream(attitude_);
            std::string temp;
            char comma;

            try
            {
                // 解析 Roll
                std::getline(attitude_stream, temp, ':');
                attitude_stream >> roll;
                attitude_stream >> comma; // 读取逗号

                // 解析 Pitch
                std::getline(attitude_stream, temp, ':');
                attitude_stream >> pitch;
                attitude_stream >> comma; // 读取逗号

                // 解析 Yaw
                std::getline(attitude_stream, temp, ':');
                attitude_stream >> yaw;
                yaw = 1.570796327 - yaw; // 根据 Python 代码中的逻辑反转 yaw
            }
            catch (const std::exception &e)
            {
                RCLCPP_ERROR(this->get_logger(), "Failed to parse attitude string: %s", e.what());
                return;
            }

            // 将欧拉角转换为四元数
            tf2::Quaternion quat;
            quat.setRPY(roll, pitch, yaw);

            t.transform.rotation.x = quat.x();
            t.transform.rotation.y = quat.y();
            t.transform.rotation.z = quat.z();
            t.transform.rotation.w = quat.w();

            tf_broadcaster_->sendTransform(t);
        }

        // 发布 base_link 到 target 的 TF
        if (current_gps_ && target_gps_ && local_pose_)
        {
            try
            {
                int zone_current, zone_target;
                bool northp_current, northp_target;
                double easting_current, northing_current, easting_target, northing_target;

                // 转换当前 GPS 到 UTM
                GeographicLib::UTMUPS::Forward(
                    current_gps_->latitude, current_gps_->longitude,
                    zone_current, northp_current, easting_current, northing_current);

                // 转换目标 GPS 到 UTM
                GeographicLib::UTMUPS::Forward(
                    target_gps_->latitude, target_gps_->longitude,
                    zone_target, northp_target, easting_target, northing_target);

                // 检查 UTM 区域和半球是否一致
                if (zone_current != zone_target || northp_current != northp_target)
                {
                    RCLCPP_ERROR_THROTTLE(
                        this->get_logger(),
                        *this->get_clock(),
                        1000,
                        "Current and target positions are in different UTM zones or hemispheres.");
                    return;
                }

                // 计算相对坐标
                double dx = easting_target - easting_current + local_pose_->pose.position.y;
                double dy = northing_target - northing_current + local_pose_->pose.position.x;
                double dz = target_gps_->altitude - current_gps_->altitude;

                // 创建 TF 消息
                geometry_msgs::msg::TransformStamped target_tf;
                if (hasClock && clock_msg_)
                {
                    target_tf.header.stamp = clock_msg_->clock;
                }
                else
                {
                    target_tf.header.stamp = this->get_clock()->now();
                }
                target_tf.header.frame_id = "odom";
                target_tf.child_frame_id = "target";
                target_tf.transform.translation.x = dx;
                target_tf.transform.translation.y = dy;
                target_tf.transform.translation.z = dz;

                // 设置单位四元数（无旋转）
                target_tf.transform.rotation.x = 0.0;
                target_tf.transform.rotation.y = 0.0;
                target_tf.transform.rotation.z = 0.0;
                target_tf.transform.rotation.w = 1.0;

                // 发布 TF
                tf_broadcaster_->sendTransform(target_tf);
            }
            catch (const GeographicLib::GeographicErr &e)
            {
                RCLCPP_ERROR_THROTTLE(
                    this->get_logger(),
                    *this->get_clock(),
                    1000,
                    "UTM conversion error: %s",
                    e.what());
            }
        }

        // 新增多目标处理逻辑
        if (current_gps_ && local_pose_ && !mult_target_gps_coords_.empty())
        {
            geometry_msgs::msg::PoseArray pose_array;
            pose_array.header.frame_id = "odom";
            if (hasClock && clock_msg_)
            {
                pose_array.header.stamp = clock_msg_->clock;
            }
            else
            {
                pose_array.header.stamp = this->get_clock()->now();
            }

            int index = 0;
            for (const auto &target : mult_target_gps_coords_)
            {
                try
                {
                    // 获取目标点坐标
                    double lat = std::get<0>(target);
                    double lon = std::get<1>(target);
                    double alt = std::get<2>(target);

                    // UTM转换
                    int zone_current, zone_target;
                    bool northp_current, northp_target;
                    double easting_current, northing_current, easting_target, northing_target;

                    GeographicLib::UTMUPS::Forward(current_gps_->latitude, current_gps_->longitude,
                                                   zone_current, northp_current, easting_current, northing_current);
                    GeographicLib::UTMUPS::Forward(lat, lon,
                                                   zone_target, northp_target, easting_target, northing_target);

                    // 区域校验
                    if (zone_current != zone_target || northp_current != northp_target)
                    {
                        RCLCPP_WARN(this->get_logger(), "Target %d UTM zone mismatch", index);
                        continue;
                    }

                    // 计算相对坐标（注意坐标轴转换）
                    double dx = easting_target - easting_current + local_pose_->pose.position.y;
                    double dy = northing_target - northing_current + local_pose_->pose.position.x;
                    double dz = 0.0;

                    // 发布TF
                    geometry_msgs::msg::TransformStamped tf;
                    tf.header = pose_array.header;
                    tf.child_frame_id = "target_" + to_string(index);
                    tf.transform.translation.x = dx;
                    tf.transform.translation.y = dy;
                    tf.transform.translation.z = dz;

                    // 设置单位四元数（无旋转）
                    tf.transform.rotation.x = 0.0;
                    tf.transform.rotation.y = 0.0;
                    tf.transform.rotation.z = 0.0;
                    tf.transform.rotation.w = 1.0;
                    tf_broadcaster_->sendTransform(tf);

                    // 添加坐标到PoseArray
                    geometry_msgs::msg::Pose pose;
                    pose.position.x = dx;
                    pose.position.y = dy;
                    pose.position.z = dz;
                    pose.orientation.w = 1.0;
                    pose_array.poses.push_back(pose);

                    index++;
                }
                catch (const GeographicLib::GeographicErr &e)
                {
                    RCLCPP_ERROR(this->get_logger(),
                                 "Target %d UTM conversion failed: %s", index, e.what());
                }
            }

            // 发布多目标坐标
            mult_pose_publisher_->publish(pose_array);
        }
    }
};

// 辅助函数：绑定 CPU 核心
bool bind_to_cpu(int cpu_core)
{
    cpu_set_t cpuset;
    CPU_ZERO(&cpuset);
    CPU_SET(cpu_core, &cpuset);
    pid_t pid = getpid();

    if (sched_setaffinity(pid, sizeof(cpu_set_t), &cpuset) == -1)
    {
        std::cerr << "Failed to set CPU affinity to core " << cpu_core << ": " << strerror(errno) << std::endl;
        return false;
    }
    else
    {
        std::cout << "Process is bound to CPU core " << cpu_core << std::endl;
        return true;
    }
}

int main(int argc, char **argv)
{
    int cpu_core = -1;
    // 解析命令行参数，查找 --cpu 参数
    for (int i = 1; i < argc; ++i)
    {
        if (strcmp(argv[i], "--cpu") == 0 && i + 1 < argc)
        {
            cpu_core = atoi(argv[i + 1]);
            // 移除 --cpu 和其值从 argv
            for (int j = i; j + 2 <= argc; ++j)
            {
                argv[j] = argv[j + 2];
            }
            argc -= 2;
            break;
        }
    }

    // 如果指定了 CPU 核心，则尝试绑定
    if (cpu_core != -1)
    {
        if (!bind_to_cpu(cpu_core))
        {
            std::cerr << "Continuing without binding to CPU core." << std::endl;
        }
    }
    else
    {
        std::cerr << "No CPU core specified or failed to parse --cpu argument." << std::endl;
    }

    // 初始化 ROS2
    rclcpp::init(argc, argv);
    auto node = std::make_shared<TFConverterNode>();
    rclcpp::spin(node);
    rclcpp::shutdown();
    return 0;
}
